#pragma once
#include "gtest/gtest.h"
#include <cmath>
#include "DataStructure/Input/Property/PRODData.h"
#include "../../Factory/ElementCalculatorFactory.h"
#include "../../Register/ElementCalculatorRegister.h"
#include "../RodCalculator.h"

using SG::Algebra::Matrixd;
using SG::Algebra::Vector3D;
using SG::Algebra::PointArray;
using SG::DataStructure::Common::IdArray;
using SG::DataStructure::Common::Real;
using namespace SG::DataStructure::FEM;
using namespace SG::FEMSolver::Element;

// ElementCalculatorFactory 是否正确创建 Rod 单元计算类
TEST (ElementCalculatorFactoryTest, CreateRodElement)
{
    elementCalculatorRegister ();

    // Test data
    const auto calculatorPtr = ElementCalculatorFactory::Create (ElementTypes::ELETYPE_Link);
    const auto rodCalculatorPtr = dynamic_cast<RodCalculator*> (calculatorPtr.get ());

    // Check
    ASSERT_TRUE (nullptr != rodCalculatorPtr);
}

// ElementCalculatorFactory 是否正确创建 Conrod 单元计算类
TEST (ElementCalculatorFactoryTest, CreateConrodElement)
{
    elementCalculatorRegister ();

    // Test data
    const auto calculatorPtr = ElementCalculatorFactory::Create (ElementTypes::ELETYPE_LinkNoRef);
    const auto rodCalculatorPtr = dynamic_cast<RodCalculator*> (calculatorPtr.get ());

    // Check
    ASSERT_TRUE (nullptr != rodCalculatorPtr);
}

TEST (RodCalculatorTest, ComputeLinkStiffness)
{
    // Input data
    const Vector3D xVec{1.0, 0.0, 0.0};
    Matrixd T (2, 6);
    T (0, 0) = xVec.m_x;
    T (1, 3) = xVec.m_x;
    T (0, 1) = xVec.m_y;
    T (1, 4) = xVec.m_y;
    T (0, 2) = xVec.m_z;
    T (1, 5) = xVec.m_z;
    const Real S{2.38};
    const Real L{10.0};
    IsotropicMaterial material;
    material.m_E = 1.0;

    // Test data
    Matrixd kgTest (6, 6);
    RodCalculator::ComputeLinkStiffness (L, material, S, T, _OUT kgTest);

    // Target data
    const Matrixd kgTarget (6, 6, {
     0.2380000, 0.0000000, 0.0000000, -0.2380000, 0.0000000, 0.0000000,
     0.0000000, 0.0000000, 0.0000000,  0.0000000, 0.0000000, 0.0000000,
     0.0000000, 0.0000000, 0.0000000,  0.0000000, 0.0000000, 0.0000000,
    -0.2380000, 0.0000000, 0.0000000,  0.2380000, 0.0000000, 0.0000000,
     0.0000000, 0.0000000, 0.0000000,  0.0000000, 0.0000000, 0.0000000,
     0.0000000, 0.0000000, 0.0000000,  0.0000000, 0.0000000, 0.0000000
    });

    // Check
    ASSERT_TRUE (kgTarget == kgTest);
}

TEST (RodCalculatorTest, ComputeTorsionStiffness)
{
    // Input data
    const Vector3D xVec{0.0, 1.0, 0.0};
    Matrixd T (2, 6);
    T (0, 0) = xVec.m_x;
    T (1, 3) = xVec.m_x;
    T (0, 1) = xVec.m_y;
    T (1, 4) = xVec.m_y;
    T (0, 2) = xVec.m_z;
    T (1, 5) = xVec.m_z;
    const Real J{9.45};
    const Real L{7.0};
    IsotropicMaterial material;
    material.m_G = 1.0;

    // Test data
    Matrixd ktTest (6, 6);
    RodCalculator::ComputeTorsionStiffness (L, material, J, T, _OUT ktTest);

    // Target data
    const Matrixd ktTarget (6, 6, {
    0.0000000,  0.0000000, 0.0000000, 0.0000000,  0.0000000, 0.0000000,
    0.0000000,  1.3500000, 0.0000000, 0.0000000, -1.3500000, 0.0000000,
    0.0000000,  0.0000000, 0.0000000, 0.0000000,  0.0000000, 0.0000000,
    0.0000000,  0.0000000, 0.0000000, 0.0000000,  0.0000000, 0.0000000,
    0.0000000, -1.3500000, 0.0000000, 0.0000000,  1.3500000, 0.0000000,
    0.0000000,  0.0000000, 0.0000000, 0.0000000,  0.0000000, 0.0000000
    });

    // Check
    ASSERT_TRUE (ktTarget == ktTest);
}

TEST (RodCalculatorTest, ComputeStiffness)
{
    // Input data
    const Real Q3{std::sqrt (3.0) / 3.0};
    const Vector3D xVec{Q3, Q3, Q3};
    Matrixd T (2, 6);
    T (0, 0) = xVec.m_x;
    T (1, 3) = xVec.m_x;
    T (0, 1) = xVec.m_y;
    T (1, 4) = xVec.m_y;
    T (0, 2) = xVec.m_z;
    T (1, 5) = xVec.m_z;

    const Real L{std::sqrt (3.0)};
    const Real S{10.0};
    IsotropicMaterial material;
    material.m_E = 2.0;

    // Test data
    Matrixd kgTest (6, 6);
    RodCalculator::ComputeLinkStiffness (L, material, S, T, _OUT kgTest);

    // Target data
    const Matrixd kgTarget (6, 6, {
     3.8490018,  3.8490018,  3.8490018, -3.8490018, -3.8490018, -3.8490018,
     3.8490018,  3.8490018,  3.8490018, -3.8490018, -3.8490018, -3.8490018,
     3.8490018,  3.8490018,  3.8490018, -3.8490018, -3.8490018, -3.8490018,
    -3.8490018, -3.8490018, -3.8490018,  3.8490018,  3.8490018,  3.8490018,
    -3.8490018, -3.8490018, -3.8490018,  3.8490018,  3.8490018,  3.8490018,
    -3.8490018, -3.8490018, -3.8490018,  3.8490018,  3.8490018,  3.8490018
    });

    // Check
    ASSERT_TRUE (kgTarget == kgTest);
}

// TEST (RodCalculatorTest, TemperatureLoadTest)
// {
//     // Input data
//     Vector3D xVec;
//     xVec.m_x = 0.0;
//     xVec.m_y = 1.0;
//     xVec.m_z = 0.0;

//     const Real S{10.0};
//     const Real deltaT{10.0};

//     IsotropicMaterial material;
//     material.m_E = 1.0;
//     material.m_A = 1.0;

//     // Test data
//     Matrixd Pe0Test (6, 1);

//     RodCalculator::ComputeTemperatureLoad (material, S, deltaT, xVec, _OUT Pe0Test);

//     const Matrixd Pe0Target (6, 1, {

//     });

    // ASSERT_TRUE (Pe0Target == Pe0Test);
// }

TEST (RodCalculatorTest, ComputeLocalMass)
{
    // Input data
    IdArray dispCoordIdSet{0, 0};
    PointArray pointSet;
    IsotropicMaterial material;
    material.m_rho = 7.85e-9;
    PRODData property;
    property.m_area = 12.0;
    const Real L{50.0};
    const Vector3D xVec{1.0, 0.0, 0.0};
    const Real m{0.5 * (material.m_rho * property.m_area + property.m_nsm) * L};

    // Test data
    Matrixd lumpedMTest (6, 6);
    RodCalculator::ComputeLumpMass (m, _OUT lumpedMTest);
    Matrixd coupledMTest (6, 6);
    RodCalculator::ComputeCoupleMass (m, xVec, dispCoordIdSet, pointSet, _OUT coupledMTest);

    // Target data
    Matrixd lumpedMTarget (6, 6);
    for (size_t i{0}; i < 6; ++i)
    {
        lumpedMTarget (i, i) = 2.35500000E-06;
    }

    const Matrixd coupledMTarget (6, 6, {
    1.96250000E-06,  0.00000000E+00,  0.00000000E+00, 3.92500000E-07,  0.00000000E+00,  0.00000000E+00,
    0.00000000E+00,  2.35500000E-06,  0.00000000E+00, 0.00000000E+00,  0.00000000E+00,  0.00000000E+00,
    0.00000000E+00,  0.00000000E+00,  2.35500000E-06, 0.00000000E+00,  0.00000000E+00,  0.00000000E+00,
    3.92500000E-07,  0.00000000E+00,  0.00000000E+00, 1.96250000E-06,  0.00000000E+00,  0.00000000E+00,
    0.00000000E+00,  0.00000000E+00,  0.00000000E+00, 0.00000000E+00,  2.35500000E-06,  0.00000000E+00,
    0.00000000E+00,  0.00000000E+00,  0.00000000E+00, 0.00000000E+00,  0.00000000E+00,  2.35500000E-06
    });

    // Check
    Real factor{1.0E6};
    ASSERT_TRUE (lumpedMTarget * factor == lumpedMTest * factor);
    ASSERT_TRUE (coupledMTarget * factor == coupledMTest * factor);
}

extern std::shared_ptr<DBServiceFactory> DB_SERVICEFACTORY;
TEST (RodCalculatorTest, ComputeGlobalMass)
{
    // Input data
    DB_SERVICEFACTORY->ClearDatabase ();

    CORD2RData rectangularCoordSys{100, 0, {1.0, 2.0, 3.0}, {2.0, 4.0, 1.0}, {5.0, 1.0, 1.0}};
    DB_SERVICEFACTORY->get<ICORD2RService> ()->Add (rectangularCoordSys);
    rectangularCoordSys = {101, 100, {4.0, 3.0, 7.0}, {2.0, 6.0, 1.0}, {5.0, 1.0, 4.0}};
    DB_SERVICEFACTORY->get<ICORD2RService> ()->Add (rectangularCoordSys);

    auto& assembly = AssemblyEigen::GetInstance ();
    assembly.Clear ();
    Subcase subcase;
    assembly.Initialize (DB_SERVICEFACTORY, subcase, {1});

    auto& coordProcessor = CoordinateProcessor::GetInstance ();
    coordProcessor.Clear ();
    coordProcessor.Initialize (DB_SERVICEFACTORY);

    IdArray dispCoordIdSet{100, 101};
    PointArray pointSet{Point{0.0, 0.0, 0.0}, Point{50.0, 0.0, 0.0}};
    IsotropicMaterial material;
    material.m_rho = 7.85e-9;
    PRODData property;
    property.m_area = 12.0;
    const Real L{50.0};
    const Vector3D xVec{1.0, 0.0, 0.0};
    const Real m{0.5 * (material.m_rho * property.m_area + property.m_nsm) * L};

    // Test data
    Matrixd coupledMTest (6, 6);
    RodCalculator::ComputeCoupleMass (m, xVec, dispCoordIdSet, pointSet, _OUT coupledMTest);

    // Target data
    const Matrixd coupledMTarget (6, 6, {
     2.09846405E-06,  1.53921569E-07, -1.05772481E-07,  1.66423091E-07, -1.41874731E-07, -2.29924498E-07,
     1.53921569E-07,  2.26264706E-06,  6.34634886E-08, -9.98538548E-08,  8.51248387E-08,  1.37954699E-07,
    -1.05772481E-07,  6.34634886E-08,  2.31138889E-06,  6.86179984E-08, -5.84964502E-08, -9.48002992E-08,
     1.66423091E-07, -9.98538548E-08,  6.86179984E-08,  2.24703601E-06,  9.20386852E-08,  1.49159391E-07,
    -1.41874731E-07,  8.51248387E-08, -5.84964502E-08,  9.20386852E-08,  2.27653755E-06, -1.27157526E-07,
    -2.29924498E-07,  1.37954699E-07, -9.48002992E-08,  1.49159391E-07, -1.27157526E-07,  2.14892644E-06
    });

    // Check
    const Real factor{1.0E8};
    ASSERT_TRUE (coupledMTarget * factor == coupledMTest * factor);
}

TEST (RodCalculatorTest, StrainTest)
{
    PointArray points (2);
    IsotropicMaterial material;
    CONRODData rodData;
    material.m_E = 72450;
    rodData.m_A = 282;
    Real L = 25.695659866989494;
    const Matrixd coeffMatrix (1, 2, { -1.0, 1.0 });
    Matrixd       TEle (2, 6);
    Matrixd            linkDisp (6, 1);
    TEle (0, 0) = 0.079780009955437381;
    TEle (0, 1) = 0.37749565686235015;
    TEle (0, 2) = -0.92256825170909351;
    TEle (1, 3) = 0.079780009955437381;
    TEle (1, 4) = 0.37749565686235015;
    TEle (1, 5) = -0.92256825170909351;
    linkDisp (0, 0) = 0.00572264254325912;
    linkDisp (1, 0) = -0.004075091070368762;
    linkDisp (2, 0) = 0.007660731667450745;
    linkDisp (3, 0) = 0.007404847363719085;
    linkDisp (4, 0) = -0.003945491344456402;
    linkDisp (5, 0) = 0.00786021667332876;

    // Test data
    Real axialStrain;
    Real axialStress;
    Real axialForce;
    RodCalculator::ComputeAxialForce (TEle, linkDisp, L, coeffMatrix, material, rodData, _OUT axialStrain, _OUT axialStress, _OUT axialForce);
    // Target data
    const Real eleAxialForce0 = -0.72266211452886431;
    // Check
    ASSERT_TRUE (axialForce == eleAxialForce0);
}

// TEST (RodCalculatorTest, StrainTest)
// {
//     const Real L{10.0};
//     IsotropicMaterial material;
//     material.m_E  = 210000.0;
//     material.m_nu = 0.3;
//     material.m_G  = material.m_E / (2.0 * (1.0 + material.m_nu));
//     PRODData property;
//     property.m_area                = 4.0;
//     property.m_torsionFactor       = 8.0;
//     property.m_torsionStressfactor = 2.5;

//     const Matrixd coeffMatrix (1, 2, {-1.0, 1.0});
//     const Matrixd TEle (2, 6, {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0});
//     const Matrixd eleDisplacement_t (6, 1, {0.0, 0.0, 0.0, 1.190476E-05, 0.0, 0.0});
//     const Matrixd eleDisplacement_r (6, 1, {0.0, 0.0, 0.0, 1.547619E-05, 0.0, 0.0});
    
//     const auto eleAxialDisplacementDiff = coeffMatrix * (TEle * eleDisplacement_t);
//     const Real eleAxialStrainTest{eleAxialDisplacementDiff (0, 0) / L};
//     const Real eleAxialStressTest{material.m_E * eleAxialStrainTest};
//     const Real eleAxialForceTest{eleAxialStressTest * property.m_area};
//     const auto eleTorsionalDisplacementDiff = coeffMatrix * (TEle * eleDisplacement_r);
//     const Real eleTorsionalStrainTest{eleTorsionalDisplacementDiff (0, 0) * property.m_torsionStressfactor / L};
//     const Real eleTorsionalStressTest{material.m_G * eleTorsionalStrainTest};
//     const Real eleTorqueTest{eleTorsionalStressTest * property.m_torsionFactor / property.m_torsionStressfactor};

//     EXPECT_FLOAT_EQ (1.190476E-06, eleAxialStrainTest);
//     EXPECT_FLOAT_EQ (3.869048E-06, eleTorsionalStrainTest);
//     EXPECT_FLOAT_EQ (2.500000E-01, eleAxialStressTest);
//     EXPECT_FLOAT_EQ (3.125000E-01, eleTorsionalStressTest);
//     EXPECT_FLOAT_EQ (1.000000E+00, eleAxialForceTest);
//     EXPECT_FLOAT_EQ (1.000000E+00, eleTorqueTest);
// }